﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using FarseerPhysics.Common;
using FarseerPhysics.Factories;
using FarseerPhysics.Dynamics;
using FarseerPhysics.Dynamics.Joints;
using FarseerPhysics.Dynamics.Contacts;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Graphics;

namespace FarseerPhysics.HelloWorld
{
    class MyPlayerBody : IMyBody
    {
        private World _world;     
        private Body _bodyTail;
        private RevoluteJoint _rjoint;

        //bike parameters
        const float MaxSteerAngle = Settings.Pi / 3;
        const float MaxKillValue = 1.0f; // max killing othogonal velocity
        const float SteerSpeed = 1.0f;  // steer speed  
        const float TorqeSpeed = 0.004f;  // steer speed  /0,03
        const float Horsepower = 8.0f;   // max speed
        const float acc = 0.09f;         //determines acceleration of a car
        const float dcc = 0.6f;         // decceleration of car
        const float kLinearDamping = 0.8f;
        const float kAngularDamping = 3.8f;

        Vector2 _carStartingPosition = new Vector2(11f, 20f);
        Vector2 _startingPosition = new Vector2(12f, 25f);

        float _engineSpeed;
        float _steeringAngle;
        //float _killValue = 1.0f;
        public int _bodyType1 { get; set; }
        public int _bodyType2 { get; set; }

        public Body _body { get; set; }

        public MyPlayerBody(World world)
        {
            _world = world;
        }

        public override void LoadContent()
        {           
            /*            
           _body = new Body(_world);        
           // define our shapes
           PolygonShape box = new PolygonShape(1);
           box.SetAsBox( 0.9f, 0.45f );  //offset from front and rear of the car(for proper wheel location)            
           _body.CreateFixture(box);
           _body.BodyType = BodyType.Dynamic;
           _body.LinearDamping = kLinearDamping;
           _body.AngularDamping = kAngularDamping;
           _body.Position = _carStartingPosition;
           _body.Rotation = MathHelper.ToRadians(90);
           _body.Mass = 1;          
            */

            Vertices rect1 = PolygonTools.CreateRectangle(0.2f, 0.02f);
            Vertices rect2 = PolygonTools.CreateRectangle(0.02f, 0.07f);

            Vector2 trans = new Vector2(-0f, 0f);
            rect1.Translate(ref trans);
            trans = new Vector2(0f, -0.2f);
            rect2.Translate(ref trans);

            List<Vertices> vertices = new List<Vertices>(2);
            vertices.Add(rect1);
            vertices.Add(rect2);

            _body = BodyFactory.CreateCompoundPolygon(_world, vertices, 1f);
            _body.BodyType = BodyType.Dynamic;
            _body.LinearDamping = kLinearDamping;
            _body.AngularDamping = kAngularDamping;
            _body.Position = _carStartingPosition;
            _body.Rotation = MathHelper.ToRadians(90);
            _body.Mass = 1;

            _bodyTail = BodyFactory.CreateRectangle(_world, 0.05f, 0.45f, 1f);
            _bodyTail.Position = _startingPosition;
            _bodyTail.BodyType = BodyType.Dynamic;
            _bodyTail.LinearDamping = kLinearDamping;
            _bodyTail.AngularDamping = kAngularDamping;
            _bodyTail.Position = _carStartingPosition + (new Vector2(0f, 0.2f));
            _bodyTail.Rotation = MathHelper.ToRadians(90);
            _bodyTail.Mass = 0.01f;

            _rjoint = JointFactory.CreateRevoluteJoint(_body, _bodyTail, Vector2.Zero);
            _rjoint.LowerLimit = MathHelper.ToRadians(-45);
            _rjoint.UpperLimit = MathHelper.ToRadians(45);
            _rjoint.LimitEnabled = true;
            _rjoint.LocalAnchorA = new Vector2(0.0f, 0.0f);
            _rjoint.LocalAnchorB = new Vector2(0.0f, -0.2f);
            _rjoint.MotorEnabled = true;
            _rjoint.MaxMotorTorque = 100f;

            _world.AddJoint(_rjoint);
            _body.OnCollision += MyOnCollision;

        }

        public override void Update()
        {
            //Driving            
            Transform tmp;
            _body.GetTransform(out tmp);
            Vector2 ldirection = tmp.R.Col2 * _engineSpeed;
            _body.ApplyForce(ldirection);

            Transform tmp2;
            _bodyTail.GetTransform(out tmp2);
            Vector2 rdirection = tmp.R.Col2 * _engineSpeed * 0.01f;
            _bodyTail.ApplyForce(rdirection);

            //Steering
            _body.ApplyTorque(_steeringAngle * TorqeSpeed);
            _rjoint.MotorSpeed = _steeringAngle;

            float mspeed = _steeringAngle - _rjoint.JointAngle;
            _rjoint.MotorSpeed = mspeed * SteerSpeed;          
        }

        public override void HandleKeyboard()
        {
            KeyboardState state = Keyboard.GetState();    
           
            if (state.IsKeyDown(Keys.W))
            {
                _engineSpeed = MathHelper.SmoothStep(_engineSpeed, -Horsepower, acc);
            }
            else if (state.IsKeyDown(Keys.S))
            {
                _engineSpeed = MathHelper.SmoothStep(_engineSpeed, Horsepower / 2, dcc);
            }
            else
            {
                _engineSpeed = MathHelper.Lerp(_engineSpeed, 0, acc / 2f);
            }

            if (state.IsKeyDown(Keys.D))
            {
                // _steeringAngle = MathHelper.SmoothStep(MaxSteerAngle, 0, acc); 
                _steeringAngle = MaxSteerAngle;
                //_bodyTail.ApplyTorque(SteerSpeed);
                //_rjoint.MotorSpeed = 5f;
            }
            else if (state.IsKeyDown(Keys.A))
            {
                // _steeringAngle = MathHelper.SmoothStep(-MaxSteerAngle, 0, acc); 
                _steeringAngle = -MaxSteerAngle;
                //_bodyTail.ApplyTorque(-SteerSpeed);
                // _rjoint.MotorSpeed = -5f;
            }
            else
            {
                // _bodyTail.
                _steeringAngle = 0;
            }

            if (!state.IsKeyDown(Keys.Space))
            {
                // _killValue = MathHelper.SmoothStep(_killValue, 1.0f, 0.1f);
                // if (getLateralVelocity().Length() < 0.1f) 
                //killOrthogonalVelocity(_body, _killValue);
            }
            else
            {
                //if (getLateralVelocity().Length() < 0.1f) 
                // if (getLateralVelocity().Length() < 0.1f) 
                // _killValue = MathHelper.SmoothStep(_killValue, 1.0f, 0.1f);                   
            }

            if (state.IsKeyDown(Keys.Home))
            {
                _body.Position = _carStartingPosition;
                _body.Rotation = MathHelper.ToRadians(90);
            }
           // _oldKeyState = state;
        }

        public override void Draw(SpriteBatch spriteBatch)
        {

        }
        private bool MyOnCollision(Fixture f1, Fixture f2, Contact contact)
        {
            //We want the collision to happen, so we return true.
            _bodyType1 = f1.Body.BodyId;
            _bodyType2 = f2.Body.BodyId;
            return true;
        }


    }
}
